/*
 * @Author: Jiagang Chen
 * @Date: 2021-12-05 09:56:42
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-06 03:12:35
 * @Description: ...
 * @Reference: ...
 */

#pragma once

#include "lci_slam/vilens/common_header.h"

struct CTPointToPlaneImuFactor {
    static constexpr int NumResiduals() { return 1; }

    CTPointToPlaneImuFactor(const Eigen::Vector3d &reference_point,
                            const Eigen::Vector3d &raw_target,
                            const Eigen::Vector3d &reference_normal,
                            double alpha_timestamp, double weight = 1.0)
        : raw_keypoint_(raw_target),
          reference_point_(reference_point),
          reference_normal_(reference_normal),
          alpha_timestamps_(alpha_timestamp),
          weight_(weight) {}

    template <typename T>
    bool operator()(const T *const pose_params_begin,
                    const T *const pose_params_end,
                    const T *const relpose_params, T *residual) const {
        Eigen::Map<Eigen::Quaternion<T>> quat_lidar2imu(
            const_cast<T *>(relpose_params + 3));
        Eigen::Matrix<T, 3, 1> transformed_imu =
            quat_lidar2imu * raw_keypoint_.template cast<T>();
        transformed_imu(0, 0) += relpose_params[0];
        transformed_imu(1, 0) += relpose_params[1];
        transformed_imu(2, 0) += relpose_params[2];

        Eigen::Map<Eigen::Quaternion<T>> quat_begin(
            const_cast<T *>(pose_params_begin + 3));
        Eigen::Map<Eigen::Quaternion<T>> quat_end(
            const_cast<T *>(pose_params_end + 3));
        Eigen::Quaternion<T> quat_inter =
            quat_begin.slerp(T(alpha_timestamps_), quat_end);
        quat_inter.normalize();

        Eigen::Matrix<T, 3, 1> transformed = quat_inter * transformed_imu;

        T alpha_m = T(1.0 - alpha_timestamps_);
        transformed(0, 0) += alpha_m * pose_params_begin[0] +
                             alpha_timestamps_ * pose_params_end[0];
        transformed(1, 0) += alpha_m * pose_params_begin[1] +
                             alpha_timestamps_ * pose_params_end[1];
        transformed(2, 0) += alpha_m * pose_params_begin[2] +
                             alpha_timestamps_ * pose_params_end[2];

        residual[0] =
            weight_ *
            (reference_point_.template cast<T>() - transformed).transpose() *
            reference_normal_.template cast<T>();

        return true;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Eigen::Vector3d raw_keypoint_;
    Eigen::Vector3d reference_point_;
    Eigen::Vector3d reference_normal_;
    double alpha_timestamps_;
    double weight_ = 1.0;
};
